#include "trajectorycapture.h"
#include <mlgesture.h>
#include <gestureutils.h>

#include <QTextStream>
#include <QFile>
#include <QStringList>
#include <map>

#include <statechangeevent.h>
#include <handsmoveevent.h>
#include <handgestureevent.h>

#define _USE_MATH_DEFINE
#include <math.h>

using namespace mlgesture;
using namespace std;

TrajectoryCapture::TrajectoryCapture(vector<Trajectory>* trajectories)
	: tracks_(trajectories)
{
	gesture_ = NULL;
	lastPos_.x = lastPos_.y = -1;
	lastDirPos_.x = lastDirPos_.y = -1;
	trackingState_ = false;
	cameraState_ = false;
	isError_ = false;
}

TrajectoryCapture::TrajectoryCapture(int cam_id, vector<Trajectory>* trajectories)
	: tracks_(trajectories)
{
	gesture_ = NULL;
	lastPos_.x = lastPos_.y = -1;
	lastDirPos_.x = lastDirPos_.y = -1;
	trackingState_ = false;
	cameraState_ = false;
	isError_ = false;
	setParams(cam_id);
}

TrajectoryCapture::~TrajectoryCapture() {
	if (gesture_)
		delete gesture_;
}

void TrajectoryCapture::setParams(int cam_id, int low_skin, int upper_skin, int low_value, int upper_value,
								  bool adapt_algorithm, double tracking_sensity) {
	if (gesture_) {
		delete gesture_;
		track_ = Mat();
	}
	gesture_ = new MLGesture(cam_id, 1);
	if (low_skin >= 0 && upper_skin >= 0 && low_value >=0 && upper_value >=0){
		gesture_->adjustSkinFilter(low_skin, upper_skin, low_value, upper_value);
	}
	if (tracking_sensity >=0 && tracking_sensity <= 1) {
		gesture_->adjustTrackingSensity(tracking_sensity);
	}
	gesture_->setAdaptiveAlgoritm(adapt_algorithm);
	gesture_->enableTrajectory(true);
	trackingState_ = false;
}

void TrajectoryCapture::frameProcess() {
	if (!gesture_) {
		throw HandGestureException("Camera not initialized");
	}
	if (isError_) {
		isError_ = false;
		throw HandGestureException(errorMessage_.toStdString());
	}
	while(!gesture_->getFrame(frame_));
	gesture_->getMask(mask_);
	if (track_.data == NULL) {
		track_ = Mat::zeros(frame_.rows, frame_.cols, CV_8UC1);
	}
	if (direction_.data == NULL) {
		direction_ = Mat::zeros(frame_.rows, frame_.cols, CV_8UC1);
	}

	render();
}

void TrajectoryCapture::trajectoryProcess() {
	if (!gesture_) {
		throw HandGestureException("Camera not initialized");
	}
	if (!cameraState_)
			return;
	if (isError_) {
		isError_ = false;
		throw HandGestureException(errorMessage_.toStdString());
	}
	if (gestureState_) {
		vector<bool> tracking_status, tracking_end;
		const vector<Trajectory>& trajectory = gesture_->getTrajectory(tracking_status, tracking_end);
		Point act_pos = gesture_->getLastPos()[0];
		int	last_dir = gesture_->getLastDirection()[0];

		if (lastPos_.x >= 0) {
			if (!trackingState_ && tracking_status[0]) {
				trackingState_ = true;
				track_ = Mat::zeros(frame_.rows, frame_.cols, CV_8UC1);
				direction_ = Mat::zeros(frame_.rows, frame_.cols, CV_8UC1);
				lastDirPos_ = Point(frame_.cols/2, frame_.rows/2);
				circle(direction_, lastDirPos_, 20, Scalar(255));
			}
			else if (trackingState_ && !tracking_status[0]) {
				trackingState_ = false;
				if (tracking_end[0]) {
					tracks_->push_back(trajectory[0]);
				}
			}
			if (trackingState_) {
				Point pos = Point(lastDirPos_.x+20*cos(last_dir*(M_PI/8)), lastDirPos_.y-20*sin(last_dir*(M_PI/8)));
				line(track_, lastPos_, act_pos, Scalar(255));
				line(direction_, lastDirPos_, pos, Scalar(255));

				lastDirPos_ = pos;
			}
		}
		else {
			lastDirPos_ = Point(frame_.cols/2, frame_.rows/2);
		}
		lastPos_ = act_pos;
	}
}

void TrajectoryCapture::render() {
	if (!gesture_) {
		throw HandGestureException("Camera not initialized");
	}
	frameImg_ = GestureUtils::convertMat2QImage(frame_);
	maskImg_ = GestureUtils::convertMat2QImage(mask_);
	trackImg_ = GestureUtils::convertMat2QImage(track_);
	directionImg_ = GestureUtils::convertMat2QImage(direction_);
}

const QImage& TrajectoryCapture::getFrame() const {
	return frameImg_;
}

const QImage& TrajectoryCapture::getMask() const {
	return maskImg_;
}

const QImage& TrajectoryCapture::getTrajectory() const {
	return trackImg_;
}

const QImage& TrajectoryCapture::getDirection() const {
	return directionImg_;
}

vector<Trajectory>* TrajectoryCapture::getTracks() {
	return tracks_;
}

void TrajectoryCapture::stopTracking() {
	if (gesture_) {
		gesture_->stopProcess();
		gesture_->releaseCamera();
	}
	cameraState_ = false;
	gestureState_ = false;
	MLGesture::removeObserver(this);
}

void TrajectoryCapture::activateTracking() {
	MLGesture::addObserver(this);
	cameraState_ = true;
	gesture_->startProcess();
}

bool TrajectoryCapture::getTrackingState() const {
	return gestureState_;
}

void TrajectoryCapture::saveShape(const QString path) const {
	if (!gesture_) {
		throw HandGestureException("Camera not initialized");
	}
	vector<Mat> shapes;
	gesture_->getTrackShapeMask(shapes);
	GestureUtils::convertMat2QImage(shapes[0]).save(path);
}

void TrajectoryCapture::processEvent(GestureEvent *event) {
	StateChangeEvent *state_event;
	switch(event->getType()) {
	case GestureEvent::STATE_EVENT:
		state_event = dynamic_cast<StateChangeEvent*>(event);
		gestureState_ = state_event->getState();
		break;
	default:
		break;
	}
}
